#ifndef __FOC_H__
#define __FOC_H__

#include "main.h"
#include "bsp_iic.h"
#include "pid.h"
#include "math.h"
#include "i2c.h"
#include "tim.h"

typedef struct
{
    float Speed;
	float Umax;
	float Ua;
	float Ub;
	float Uc;
	float U_alpha;
	float U_beta;
	float Uq;
	float Ud;
	uint8_t pole;
	float motor_angle;
    float last_motor_angle;
	float current_angle;
	float rate;
	pid_controller FOC_S_PID;
	pid_controller FOC_P_PID;
	I2C_HandleTypeDef* i2c;
}FOC_TypeDef;

extern FOC_TypeDef FOC_1;
extern FOC_TypeDef FOC_2;

#define PI 3.1415926

void PWM_init();

void FOC_Init(FOC_TypeDef* motor,I2C_HandleTypeDef* i2c, float Umax, uint8_t pole, float rate);

void FOC_Position_Pid(FOC_TypeDef* motor, float target_angle);
void FOC_Speed_Pid(FOC_TypeDef* motor, float target_angle);

void FOC_Position_exe(FOC_TypeDef* motor,float target_angle);
void FOC_Speed_exe(FOC_TypeDef* motor,float target_angle);
/**
           Ua
           ^
           |                                                    
           |
      120  |  120      
           O
          / \
         /   \
        / 120 \
       v       v
      Ub       Uc
  @brief:三相坐标系（实际计算要顺时针旋转90度）

       U_beta
       ^
       |
       |
       |
       |
       O-------->U_alpha
  @brief:克拉克变换坐标系                
					 
       U_beta
       ^   Ud
 Uq    |   ^
  ^    |  /
   ╲  | /
     ╲|/ current_angle
       O-------->U_alpha
  @brief:帕克变换坐标系(Uq,Ud夹角90）

*/

#endif
